/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/rp_ui/qnode.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace rp_ui {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
	init_argc(argc),
	init_argv(argv)
	{}

QNode::~QNode() {
    if(ros::isStarted()) {
      ros::shutdown();
      ros::waitForShutdown();
    }
	wait();
}

/**
 * Trunk callback. Emit the updated signal.
 */
void QNode::trunkCallback(const comm::trunk::ConstPtr &msg)
{
	emit trunkUpdated(msg->state);
}

/**
 * Gripper callback. Emit the updated signal.
 */
void QNode::gripperCallback(const comm::gripper::ConstPtr &msg)
{
	emit gripperUpdated(msg->state);
}

/**
 * Eyebrows callback. Emit the updated signal.
 */
void QNode::eyebrowsCallback(const comm::eyebrows::ConstPtr &msg)
{
	emit eyebrowsUpdated(msg->state);
}

/**
 * Ears callback. Emit the updated signal.
 */
void QNode::earsCallback(const comm::ears::ConstPtr &msg)
{
	emit earsUpdated(msg->state);
}

/**
 * Sends a message to the trunk.
 */
void QNode::sendTrunk(int msg)
{
	comm::trunk trunk_msg;
	trunk_msg.state = msg;
	trunk_pub.publish(trunk_msg);
}

/**
 * Sends a message to the gripper.
 */
void QNode::sendGripper(int msg)
{
	comm::gripper gripper_msg;
	gripper_msg.state = msg;
	gripper_pub.publish(gripper_msg);
}

/**
 * Send a message to the eyebrows.
 */
void QNode::sendEyebrows(int msg)
{
	comm::eyebrows eyebrows_msg;
	eyebrows_msg.state = msg;
	eyebrows_pub.publish(eyebrows_msg);
}

/**
 * Send a message to the ears.
 */
void QNode::sendEars(int msg)
{
	comm::ears ears_msg;
	ears_msg.state = msg;
	ears_pub.publish(ears_msg);
}

bool QNode::init() {
	ros::init(init_argc,init_argv,"rp_ui");
	if ( ! ros::master::check() ) {
		return false;
	}
	subscribeTopics();
	start();
	return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
	std::map<std::string,std::string> remappings;
	remappings["__master"] = master_url;
	remappings["__hostname"] = host_url;
	ros::init(remappings,"test");
	if ( ! ros::master::check() ) {
		return false;
	}
	subscribeTopics();
	start();
	return true;
}

void QNode::subscribeTopics()
{
    ros::start();
    ros::NodeHandle n;
	// publishers
	trunk_pub = n.advertise<comm::trunk>(Topic::MOTOR_TRUNK, 10);
	gripper_pub = n.advertise<comm::gripper>(Topic::MOTOR_GRIPPER, 10);
	eyebrows_pub = n.advertise<comm::eyebrows>(Topic::MOTOR_BROW, 10);
	ears_pub = n.advertise<comm::ears>(Topic::MOTOR_EARS, 10);

	// subscribers
	trunk_sub = n.subscribe(Topic::TRUNK_STATUS, 10, &QNode::trunkCallback, this);
	gripper_sub = n.subscribe(Topic::GRIPPER_STATUS, 10, &QNode::gripperCallback, this);
	eyebrows_sub = n.subscribe(Topic::EYEBROWS_STATUS, 10, &QNode::eyebrowsCallback, this);
	ears_sub = n.subscribe(Topic::EARS_STATUS, 10, &QNode::earsCallback, this);
}

void QNode::run() {
	ros::Rate loop_rate(1);
	while ( ros::ok() ) {

		// get data from the motors

		ros::spinOnce();
		loop_rate.sleep();
	}
	std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
	Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}

}  // namespace rp_ui
